/**********************************************************************************************************************
| Project       : SCU
| File Name     : Switch.c
|
| Description   : Switch model Initialize and generate gear signal.
|                       
|                 Only ues in FM33LG045A
|                               
|----------------------------------------------------------------------------------------------------------------------
|                                   C O P Y R I G H T
|----------------------------------------------------------------------------------------------------------------------
|   Copyright (c) by Zhejiang WanChao Electric .     All rights reserved.
|
|   This software is copyright protected and proprietary to Zhejiang WanChao Electric.
|
|   Zhejiang WanChao Electric grants to you only those rights as set out in the license conditions.  
|   
|   All other rights remain with Zhejiang WanChao Electric.
|
|----------------------------------------------------------------------------------------------------------------------
|                               A U T H O R   I D E N T I T Y
|----------------------------------------------------------------------------------------------------------------------
| Initials     Name                      Company
| --------     ---------------------     ------------------------------------------------------------------------------
| HAM           He Ai Min             Zhejiang WanChao Electric.
|----------------------------------------------------------------------------------------------------------------------
|               R E V I S I O N   H I S T O R Y
|----------------------------------------------------------------------------------------------------------------------
| Date       Version   Author  Description
|----------  --------  ------  ----------------------------------------------------------------------------------------
|2024-10-01  S1.0      HAM     - Creation
|2024-10-29  S1.0      HAM     - modification

    [..] 档位状态:
       (+) 光电GPIO：High or low level.
       (+) 光电组合逻辑值：{0b1111, 0b1001, 0b1100, 0b0110, 0b0011}
	     (+) 换挡器物理位置：	
							POINT_0_RST, // 0点位置
							POINT_1_POS, // 1点位置
							POINT_2_POS, // 2点位置
							POINT_1_NEG, //-1点位置
							POINT_2_NEG, //-2点位置
							POINT_INIT,	 //  初始值
       (+) 换挡器动作： MovNone, MovTo_0, MovTo_Z_1, MovTo_Z_2, MovTo_F_1, MovTo_F_2
       (+) 物理档位值： GEARS_INIT, GEARS_N, GEARS_D, GEARS_R, GEARS_P
	     (+) CAN信号档位值： GEARS_INIT, GEARS_N, GEARS_D, GEARS_R, GEARS_P
			 
		[..] 版本修改：
		(+) 
		(+) 
		(+) 
	   
|*************************************************************************************************************************/

/*************************************************************************************************************************
Include File Segment
**************************************************************************************************************************/
#include "Switch.h"
#include "GpioAbst.h"
#include "canfd.h"
// #include "IF_Analog.h"
/*************************************************************************************************************************
Const Define Segment
**************************************************************************************************************************/

/*************************************************************************************************************************
Function Declare Segment
**************************************************************************************************************************/
static void SwitchMechDiagDetected(void);
/* 光电真值表,与PhoPointIdx对应. 				  0:POINT_0_RST, 1:POINT_1_POS, 2:POINT_2_POS, 3:POINT_1_NEG, 4:POINT_2_NEG */
											/* 中心位置、上拨1档、上拨2档、下拨1档、下拨2档 */
static uint8_t PhotoIntValue[HALL_NUM] = {0b1001, 0b0011, 0b0110, 0b1100, 0b0000};//霍尔1-bit3, 霍尔2-bit2, 霍尔3-bit1, 霍尔4-bit0
volatile int8_t _GpioInput[SWITCH_STRUCT_SIZE] = {0};
volatile InputT gInput;
extern uint8 flag_rx230;
/*************************************************************************************************************************
 Global Variable Define Segment
**************************************************************************************************************************/
SwitchStruct Switch[] = 
{    
    /*HAL_1 ***********************************************************/
    {PHOTO_INTER_1_PORT, PHOTO_INTER_1_PIN,PHOTO_INTER_1_ON_LV,PHOTO_INTER_1_FLT_TIMES,CLEAR,Release,},
    
    /*HAL_2 ***********************************************************/
    {PHOTO_INTER_2_PORT, PHOTO_INTER_2_PIN,PHOTO_INTER_2_ON_LV,PHOTO_INTER_2_FLT_TIMES,CLEAR,Release,},
    
    /*HAL_3 ***********************************************************/
    {PHOTO_INTER_3_PORT, PHOTO_INTER_3_PIN,PHOTO_INTER_3_ON_LV,PHOTO_INTER_3_FLT_TIMES,CLEAR,Release,},
    
    /*HAL_4 ***********************************************************/
    {PHOTO_INTER_4_PORT, PHOTO_INTER_4_PIN,PHOTO_INTER_4_ON_LV,PHOTO_INTER_4_FLT_TIMES,CLEAR,Release,},

    /*Park switch ***********************************************************/
    {PARK_SW_PORT, PARK_SW_PIN,PARK_SW_ON_LV,PARK_SW_FLT_TIMES,CLEAR,Release,},	

    /*KL15 ***********************************************************/
    {IG_PORT, IG_PORT_PIN,IG_ON_LV,IG_FLT_TIMES,CLEAR,Release,},		
};

SwitchDiag_ST   SwitchDiag = {0};
PhotoInt_ST PhoInt = {POINT_0_RST, 0xff, 0xff, INACTIVE, MovNone, 0x00, 0xff, 0x00, {0xff,0xff,0xff,0xff,0xff}, 0x00};
Machine_ST MachSt = {0};
uint8_t CurMachPos = POINT_0_RST, PreMachPos = POINT_0_RST;
uint8_t VcuGear = 10;//初始值//
static uint16_t MechResCnt = 0;

static uint16_t PgearResCnt = 0;
static uint16_t PgearErrCnt = 0;
static uint8_t PgearErrFlag = 0;

static uint8_t PhoInitOver = FALSE;				//拨杆位置初始化标志//
static uint8_t HallErrConfirmDelayms = 0;		//上电霍尔值异常确认时间
static uint8_t InitPostState = 0xff;			//上电初始霍尔值
static uint8_t HandlePosition = POINT_0_RST;	//手柄位置
static uint8_t HandlePosFrmCnt = 0;				//手柄位置请求帧数

static uint8_t CanTx_ShiftPos = 0;              //换挡器位置CAN信号
static uint8_t CanTx_PButtonLongPress = 0;		//P挡长按信号
static uint8_t CanTx_ParkButtonSts_VD = 0;		//P挡信号有效
static uint8_t CanTx_ParkButtonSts = 0;			//P挡短按信号
static uint16_t ParkButtonPressCnt = 0;

static uint8_t GearInputFlt, PostnInputFlt;		//档位值，手柄位
static uint8_t GearPreInput, PostnPreInput;		//前档位值，前手柄位置
static uint8_t GearInputFltCnt,PostnInputFltCnt;//档位值计时器，手柄位置计时器
static uint8_t hall_scan_complete = 0;			//上电霍尔扫描完成标记
static uint8_t hall_fault[4] = {0,0,0,0};		//4霍尔故障状态 	0:无  1:故障
/*************************************************************************************************************************
 funtion Define Segment
**************************************************************************************************************************/
uint8_t ComparePhoIntValue(PhotoInt_ST* pPhoInt, const uint8_t* TableValue);
uint8_t ScanHallSensor(void);
uint8_t only_one_unmatched(void);
uint8_t CountOnes(uint8_t byte, uint8_t *idx);
uint8_t calc_error_sensor(void);
void GearEvent(int8_t* MovtoPostion);
void Gear_Goto_Park(void);
void Gear_Goto_R(void);
void Gear_Goto_N(void);
void Gear_Goto_D(void);
#define InitInputData()    do { GearInputFlt = 0x0f; GearPreInput = 0xff; PostnPreInput = 0xff; \
								PostnInputFlt = 0x0f; GearInputFltCnt = 0; PostnInputFltCnt = 0; \
								gInput.Value = 0x0f; memset(_GpioInput, 0, sizeof(_GpioInput)); hall_scan_complete = 0; \
							}while(0)


void CanSignalInit(void)
{
	ParkButtonPressCnt = 0;
	CanTx_ShiftPos = 0;             //换挡器位置CAN信号
	CanTx_PButtonLongPress = 0;
	CanTx_ParkButtonSts_VD = 0;
}
/***********************************************************************************************************************
* Function Name: Switch_ScanInput
* Description  :
* Arguments    :
* Return Value : None
***********************************************************************************************************************/
void IF_Switch_Init(void)
{
	uint8_t i;
	
	PhoInitOver = FALSE;
	InitPostState = 0xff;
	HallErrConfirmDelayms = 0;

	PhoInt.MachPosIdx = POINT_INIT;
	PhoInt.CommbValue.Value = 0xff;
	PhoInt.IncCnt = MovNone;
	PhoInt.ScuGearEn = INACTIVE;
	PhoInt.ErrPositionCode = 0xff;
	PhoInt.ErrorPositionCount = 0;
	PhoInt.ErrSensorConfirmIdx = 0xff;
	PhoInt.ErrSensorIndex = 0xff;
    PhoInt.PreHallValue = 0xff;
	PhoInt.PassPositionFlag = 0;
	for(i=0;i<5;i++)
	{
		PhoInt.ErrorVaule[i] = 0xff;
		PhoInt.ConfirmPosition[i] = 0;
	}

	CurMachPos = POINT_0_RST;
	PreMachPos = POINT_0_RST;
	HandlePosition = POINT_0_RST;

	VcuGear = GEARS_P;
	Gear_Goto_Park();
//	IF_CanVcuStateInit();
	
	PgearErrCnt = 0;
	PgearErrFlag = 0;

	SwitchDiag.MechFaultCnt = 0;
	SwitchDiag.MechFault = 0;
	Switch[IG_SIG_INDEX].Flt = Release;	//初始化KL15电平状态为OFF 2024.2.22
	
    InitInputData();
	memset(hall_fault, 0, sizeof(hall_fault));
}

/***********************************************************************************************************************
* Function Name: Switch_ScanInput
* Description  :
* Arguments    :
* Return Value : None
***********************************************************************************************************************/
uint8_t Switch_ScanInput(void)
{
	uint8_t CurInput;
	uint8_t i, Event = 0;
	volatile uint8_t input = 0;
	volatile int8_t Cnt = 0;

	input = IF_GPIO_ReadPinData(PHOTO_INTER_1_PORT, PHOTO_INTER_1_PIN) << 3;
	input |= IF_GPIO_ReadPinData(PHOTO_INTER_2_PORT, PHOTO_INTER_2_PIN) << 2;
	input |= IF_GPIO_ReadPinData(PHOTO_INTER_3_PORT, PHOTO_INTER_3_PIN) << 1;
	input |= IF_GPIO_ReadPinData(PHOTO_INTER_4_PORT, PHOTO_INTER_4_PIN) << 0;
	input |= !IF_GPIO_ReadPinData(PARK_SW_PORT, PARK_SW_PIN) << 4;
	input |= !IF_GPIO_ReadPinData(IG_PORT, IG_PORT_PIN) << 5;

	for(i = 0; i < SWITCH_STRUCT_SIZE; i++)
	{
		Cnt = (input & (1<<i)) ?
			GPIO_SCAN_PERIOD : -GPIO_SCAN_PERIOD;
		//key press
		if((_GpioInput[i] < GPIO_LEVEL_THRES) && (Cnt > 0))
		{
			_GpioInput[i] += Cnt;
			if(_GpioInput[i] >= GPIO_LEVEL_THRES)
			{
				_GpioInput[i] = GPIO_LEVEL_THRES;
				gInput.Value |=  (1<<i);
			}
		}
		//key release
		else if((_GpioInput[i] > 0) && (Cnt < 0))
		{
			_GpioInput[i] += Cnt;
			if(_GpioInput[i] <= 0)
			{
				gInput.Value &=  ~(1<<i);
			}
		}
	}

	//换挡动作，滤波时间50ms//
	CurInput = input & 0x0f;
	if(CurInput != GearPreInput)
	{
		for(i = 0; i < 5; i++) 
		{
			if(CurInput == PhotoIntValue[i]) break;
		}
		if(i < 5) {
			GearInputFltCnt = GEAR_FILTER_TIME;	  //霍尔正常时50ms
		}
		else {
			GearInputFltCnt = HAL_ERR_FILTER_TIME;//霍尔异常时100ms
		}
		GearPreInput = CurInput;
	}
	else 
	{
		if(0 != GearInputFltCnt) GearInputFltCnt -= GPIO_SCAN_PERIOD;
		if(0 == GearInputFltCnt) { GearInputFlt = CurInput; Event = 1; }
	}

	//拨杆动作，滤波时间40ms//
	if(CurInput != PostnPreInput)
	{
		PostnInputFltCnt = PADDLE_ACT_FLT_TIME - GPIO_SCAN_PERIOD;
		PostnPreInput = CurInput;
	}
	else 
	{
		if(0 != PostnInputFltCnt) PostnInputFltCnt -= GPIO_SCAN_PERIOD;
		if(0 == PostnInputFltCnt) PostnInputFlt = CurInput;
	}
	//无错误霍尔
	if(PhoInt.ErrSensorIndex == 0xff)
	{
		for(i = 0; i < 5; i++)
		{
			if(PostnInputFlt == PhotoIntValue[i])
			{	
				HandlePosition = i;
				break;
			}
		}
	}
	//有错误霍尔，忽略该霍尔
	else if(PhoInt.ErrSensorIndex < 4)
	{			
		uint8_t err_hall_mask = (~(1 << PhoInt.ErrSensorIndex)) & 0x0f;
		for(i=0; i<5; i++)
		{
			if((PostnInputFlt & err_hall_mask) == (PhotoIntValue[i] & err_hall_mask))
			{
				HandlePosition = i;
				break;
			}
		}
	}
	return Event;
}
/*************************************************************************************************************************
| NAME:
| PROTOTYPE:
| CALLED BY:
| PRECONDITIONS:
| INPUT PARAMETERS:
| RETURN VALUE:
|
|
| DESCRIPTION:
|
|*************************************************************************************************************************/
void GetSwitchState(void)
{
	uint8 i, CurrentLv;
	
    for(i = 0; i < (sizeof(Switch) / sizeof(SwitchStruct)); i++)
    {
        if((i == IG_SIG_INDEX) || (i == PARK_SW_INDEX))
            CurrentLv = !IF_GPIO_ReadPinData(Switch[i].Port, Switch[i].Pin);
		else CurrentLv = IF_GPIO_ReadPinData(Switch[i].Port, Switch[i].Pin);
        
        switch(Switch[i].Flt)
        {
			case Release    :
				if(CurrentLv == Switch[i].OnLv)
				{
					Switch[i].Flt = PressFlt;
					Switch[i].FltCnt = CLEAR;
				}
				else
				{
					//do nothing
				}
				break;

			case PressFlt   :
				if(Switch[i].FltCnt >= Switch[i].FltTime)
				{
					Switch[i].FltCnt = CLEAR;
					Switch[i].Flt = Press;
				}
				else if(CurrentLv == Switch[i].OnLv)
				{
					Switch[i].FltCnt++;
				}
				else
				{
					Switch[i].FltCnt = CLEAR;
					Switch[i].Flt = Release;
				}
				break;

			case Press      :
				if(CurrentLv != Switch[i].OnLv)
				{
					Switch[i].Flt = ReleaseFlt;
					Switch[i].FltCnt = CLEAR;
				}
				else
				{
					//do nothing
				}
				break;

			case ReleaseFlt :
				if(Switch[i].FltCnt >= Switch[i].FltTime)
				{
					Switch[i].FltCnt = CLEAR;
					Switch[i].Flt = Release;
				}
				else if(CurrentLv != Switch[i].OnLv)
				{
					Switch[i].FltCnt++;
				}
				else
				{
					Switch[i].FltCnt = CLEAR;
					Switch[i].Flt = Press;
				}
				break;

			default :
				break;
        }
    }	
}

/***********************************************************************************************************************
* Function Name: IF_SwitchScanTask_10ms
* Description  : 开关扫描任务，10ms调用一次
* Arguments    : None
* Return Value : None
***********************************************************************************************************************/
uint8_t IF_Get_PBPButtonLongPress(void)
{
	return CanTx_PButtonLongPress;
}
uint8_t IF_Get_CanTx_ParkButtonSts_VD(void)
{
	return CanTx_ParkButtonSts_VD;
}
uint8_t IF_Get_CanTx_ParkButtonSts(void)
{
	return CanTx_ParkButtonSts;
}
void ParkButtonHandle(void)
{
	if((IF_GetVehicleSpeed() > 53) || (Switch[IG_SIG_INDEX].Flt != Press)) return;
	//P按键处理
	if(Switch[PARK_SW_INDEX].Flt == Press)
	{
		if(ParkButtonPressCnt < P_BUTTON_SHORT_TIME) ParkButtonPressCnt++;
		if(ParkButtonPressCnt >= P_BUTTON_SHORT_TIME)
		{
			/* 超过45秒，按键粘连 */
			// CanTx_ParkButtonSts = 1;	//Pressed
			// CanTx_PButtonLongPress = 1; //Request
			CanTx_ParkButtonSts_VD = 1; //Invalid 粘连故障
		}
		else if((ParkButtonPressCnt >= P_BUTTON_LONG_PRESS_TIME) && (ParkButtonPressCnt < P_BUTTON_SHORT_TIME))
		{
			/* P按键长按 */
			CanTx_ParkButtonSts = 1;	//Pressed
			CanTx_PButtonLongPress = 1; //Request
			CanTx_ParkButtonSts_VD = 0; //Valid
		}
		else /* 小于5秒 */
		{
			CanTx_ParkButtonSts = 1;	//Pressed
			CanTx_PButtonLongPress = 0; //No Request
			CanTx_ParkButtonSts_VD = 0; //Valid
		}
	}
	else if(Switch[PARK_SW_INDEX].Flt == Release)
	{
		ParkButtonPressCnt = 0;
		CanTx_ParkButtonSts = 0;	//UnPressed
		CanTx_PButtonLongPress = 0; //No Request
		CanTx_ParkButtonSts_VD = 0; //Valid
	}
}
void IF_SwitchScanTask_10ms(void)
{
    uint8 i;
	uint8_t ScuGear = 0;
	uint8_t CanSig_VcuGear = 0xff;
    static uint8_t Psw_press = 0;
	uint8_t hall_scan_complete = 0;
    
	/* 1. IO采集 */
	GetSwitchState();
	/* 2. P按键处理 */
	ParkButtonHandle();
	/* 3. 霍尔电平 */
    hall_scan_complete = Switch_ScanInput();//IF_GetSwitchState(0) | (IF_GetSwitchState(1) << 1) | (IF_GetSwitchState(2) << 2) | (IF_GetSwitchState(3) << 3);
	PhoInt.CommbValue.Value = GearInputFlt;
	
	//3. 上电检测传感器
	if((FALSE == PhoInitOver) && hall_scan_complete )
	{
		uint8_t unmatchedCnt = 0;
		uint8_t mismatch_idx = 0xff;
		uint8_t PosZero_match = 0xff;

		PosZero_match = InitPostState ^ PhotoIntValue[0];

		if(PosZero_match == 0U) PhoInitOver = TRUE; //PhoInt.MachPosIdx == POINT_0_RST;
		else
		{
			if(InitPostState != PhoInt.CommbValue.Value)
			{
				HallErrConfirmDelayms = PWRON_HALL_ERR_CONFIRM_TIME;
				InitPostState = PhoInt.CommbValue.Value;
			}
			else
			{
				if(0 != HallErrConfirmDelayms) HallErrConfirmDelayms--;
				else
				{
					for(uint8 j = 0; j < 4; j++)//4个霍尔
					{
						if(PosZero_match & (1 << j)) 
						{
							unmatchedCnt++;
							mismatch_idx = j; //不匹配的霍尔位号
						}
					}
					PhoInitOver = TRUE;
				}
			}
		}
		if(PhoInitOver == TRUE)
		{
			PhoInt.ErrorPositionCount = 0;
			if(unmatchedCnt <= 1) 
			{
				PhoInt.MachPosIdx = POINT_0_RST;
				PhoInt.ScuGearEn = ACTIVE;
				if(unmatchedCnt == 1) 
				{
					PhoInt.ErrSensorIndex = mismatch_idx;
					PhoInt.ErrPositionCode = InitPostState;
					hall_fault[PhoInt.ErrSensorIndex] = 1;
				}
				else if(unmatchedCnt == 0) {
					PhoInt.PassPositionFlag = 0x1 << 0;
					PhoInt.ConfirmPosition[0] = 1;
				}
			}
			else
			{
				//记录故障
				PhoInitOver = FALSE;
				PhoInt.ScuGearEn = INACTIVE;
			}
		}
	}

	//3. 获取机械位置:比对真值表//
	if((PhoInt.ScuGearEn == ACTIVE) && (ScanHallSensor() == true)) //ComparePhoIntValue(&PhoInt, PhotoIntValue)==true. generate data: MachPosIdx
	{
		PreMachPos = CurMachPos;
		CurMachPos = PhoInt.MachPosIdx;

		if(/*(ImmoAuth_GetScuStatus() == 2U) && */(PreMachPos != CurMachPos))
		{
			//GenPositionEvent();
			switch(CurMachPos)
			{
				case POINT_0_RST:
					PhoInt.IncCnt = MovTo_0;
//					PhoInt.ScuGearEn = ACTIVE;
					break;
					
				case POINT_1_POS:
					switch(PreMachPos)
					{
						case POINT_0_RST:
							PhoInt.IncCnt = MovTo_Z_1;
							break;
						case POINT_2_POS://+2 -> +1忽略//
							break;
						case POINT_1_NEG://-1 -> +1
							PhoInt.IncCnt = MovTo_Z_1;
							break;
						case POINT_2_NEG://-2 -> +1
							PhoInt.IncCnt = MovTo_Z_1;
							break;
						case POINT_ERR:
							break;
					}
					break;
					
				case POINT_2_POS:
					switch(PreMachPos)
					{
						case POINT_0_RST:
							PhoInt.IncCnt = MovTo_Z_2;
							break;
						case POINT_1_POS: //+1 -> +2
							PhoInt.IncCnt = MovTo_Z_2;
							break;
						case POINT_2_POS:
							break;
						case POINT_1_NEG: //-1 -> +2
							PhoInt.IncCnt = MovTo_Z_2;
							break;
						case POINT_2_NEG: //-2 -> +2
							PhoInt.IncCnt = MovTo_Z_2;
							break;
						case POINT_ERR:
							break;
					}
					break;
					
				case POINT_1_NEG:
					switch(PreMachPos)
					{
						case POINT_0_RST:
							PhoInt.IncCnt = MovTo_F_1;
							break;
						case POINT_1_POS: //+1 -> -1
							PhoInt.IncCnt = MovTo_F_1;
							break;
						case POINT_2_POS: //+2 -> -1
							PhoInt.IncCnt = MovTo_F_1;
							break;
						case POINT_1_NEG:
							break;
						case POINT_2_NEG:
							break;
						case POINT_ERR:
							break;
					}
					break;
					
				case POINT_2_NEG:
					switch(PreMachPos)
					{
						case POINT_0_RST:
							PhoInt.IncCnt = MovTo_F_2;
							break;
						case POINT_1_POS: //1 -> -2
							PhoInt.IncCnt = MovTo_F_2;
							break;
						case POINT_2_POS: //2 -> -2
							PhoInt.IncCnt = MovTo_F_2;
							break;
						case POINT_1_NEG:
							PhoInt.IncCnt = MovTo_F_2;
							break;
						case POINT_2_NEG:
							break;
						case POINT_ERR:
							break;
					}
					break;
					
				case POINT_ERR:
					break;
			}
			//4. 更新物理档位 Updata Gear
			GearEvent(&PhoInt.IncCnt);
		}
	}
	
	//5. SCU档位与HCU挡位信号同步
	ScuGear = MachSt.GearCurState;			//ScuGear 	0:Init, 1:N, 2:D, 3:R, 4:P, 5:Error

//	CanSig_VcuGear = IF_CanGetVcuState();	//CanSig_VcuGear    5:D, 6:N, 7:R, 8:P, 0xff:校验错误或报文丢失
	CanSig_VcuGear = VCU_202_Msg.VCU_ActualGear;
	if(CanSig_VcuGear <= 0x0F)				//VcuGear 			0:N, 1:D, 2:R, 3:P
	{
		switch(CanSig_VcuGear)
		{
			case VCU_CAN_RX_GEAR_N: VcuGear = GEARS_N; break;
			// case 0x0A: VcuGear = GEARS_E; break;
			// case 0x0B: VcuGear = GEARS_S; break;
			case VCU_CAN_RX_GEAR_R: VcuGear = GEARS_R; break;
			case VCU_CAN_RX_GEAR_D: VcuGear = GEARS_D; break;
			case VCU_CAN_RX_GEAR_P: 
                VcuGear = GEARS_P; 
            break;
			default : break;
		}
	}
	if((VcuGear <= GEARS_P) && (VcuGear != ScuGear))
	{
		if((ScuGear > GEARS_INIT) && (ScuGear <= GEARS_P))
		{
			if(MachSt.TxReqGearDelay <= 0)
			{
				MachSt.GearCurState = VcuGear;
				MachSt.TxReqGearDelay = 0;
			}
		}
	}
	//档位发送信号超时计数//
	if(MachSt.TxReqGearDelay > 0)
	{
		MachSt.TxReqGearDelay -= SWITCH_SCAN_PERIOD;
	}
	
	//Park Switch 
	if(gInput.bits.park_sw == 1)
	{
		if(Psw_press == 0)
		{
			Psw_press = 1;
			Gear_Goto_Park();
		}
	}
	else Psw_press = 0;

	//机械卡滞检测//
	SwitchMechDiagDetected();
}

void SwitchMechDiagDetected(void)
{
	// if(IF_GetPowerState() == VOLT_NORM)
	{
		if(PhoInt.MachPosIdx != POINT_0_RST)// && (SwitchDiag.MechFaultCnt > 0))
		{
			MechResCnt = 0;
			SwitchDiag.MechFaultCnt += SWITCH_SCAN_PERIOD;
			if(SwitchDiag.MechFaultCnt >= DIAG_MECH_ERR_TIME)
			{
				SwitchDiag.MechFaultCnt = DIAG_MECH_ERR_TIME;
				SwitchDiag.MechFault = 1;
			}
		}
		else
		{
			SwitchDiag.MechFaultCnt = 0;
			MechResCnt += SWITCH_SCAN_PERIOD;
			if(MechResCnt >= 50) 
			{
				MechResCnt = 50;
				SwitchDiag.MechFault = 0;
			}
		}
		if(gInput.bits.park_sw == 1)
		{
			PgearResCnt = 0;
			PgearErrCnt += SWITCH_SCAN_PERIOD;
			if(PgearErrCnt >= DIAG_MECH_ERR_TIME)
			{
				PgearErrCnt = DIAG_MECH_ERR_TIME;
				PgearErrFlag = 1;
			}
		}
		else
		{
			PgearErrCnt = 0;
			PgearResCnt += SWITCH_SCAN_PERIOD;
			if(PgearResCnt >= 50)
			{
				PgearResCnt = 50;
				PgearErrFlag = 0;
			}
		}
	}
}
void IF_SwitchMecFaultClear(void)
{
	MechResCnt = 0;
	SwitchDiag.MechFaultCnt = 0;
	SwitchDiag.MechFault = 0;
	PgearErrCnt = 0;
	PgearResCnt = 0;
	PgearErrFlag = 0;
}

void Gear_Goto_Park(void)
{
	MachSt.GearCurState = GEARS_P;
	MachSt.TxReqGearDelay = SCU_VCU_SYN_DELAY;
}
void Gear_Goto_R(void) 
{
	MachSt.GearCurState = GEARS_R;
	MachSt.TxReqGearDelay = SCU_VCU_SYN_DELAY;
}
void Gear_Goto_N(void) 
{
	MachSt.GearCurState = GEARS_N;
	MachSt.TxReqGearDelay = SCU_VCU_SYN_DELAY;
}
void Gear_Goto_D(void) 
{
	MachSt.GearCurState = GEARS_D;
	MachSt.TxReqGearDelay = SCU_VCU_SYN_DELAY;
}
/*************************************************************************************************************************
| NAME:
| PROTOTYPE:
| CALLED BY:
| PRECONDITIONS:
| INPUT PARAMETERS:
| RETURN VALUE:
|
|
| DESCRIPTION:
|
|*************************************************************************************************************************/
void GearEvent(int8_t* MovtoPostion)
{
	switch(MachSt.GearCurState)
	{
		case GEARS_INIT:
			//if(CurMachPos == POINT_0_RST)
			{
				Gear_Goto_Park();
			}
			break;
			
		case GEARS_P:
			switch(*MovtoPostion)
			{
				case MovTo_F_1:	Gear_Goto_N(); break;
				case MovTo_F_2: Gear_Goto_D(); break;
				case MovTo_Z_1:	Gear_Goto_N(); break;//更改2023.4.8
				case MovTo_Z_2: Gear_Goto_R(); break;
			}
			break;
			
		case GEARS_R:
			switch(*MovtoPostion)
			{
				case MovTo_F_1: Gear_Goto_N(); break;
				case MovTo_F_2: Gear_Goto_D(); break;
				case MovTo_Z_1:	Gear_Goto_R(); break;
				case MovTo_Z_2: Gear_Goto_R(); break;
			}
			break;
			
		case GEARS_N:
			switch(*MovtoPostion)
			{
				case MovTo_F_1: Gear_Goto_D(); break;
				case MovTo_F_2: Gear_Goto_D(); break;
				case MovTo_Z_1:	Gear_Goto_R(); break;
				case MovTo_Z_2: Gear_Goto_R(); break;
			}	
			break;
			
		case GEARS_D:
			switch(*MovtoPostion)
			{
				case MovTo_F_1: Gear_Goto_D(); break;
				case MovTo_F_2: Gear_Goto_D(); break;
				case MovTo_Z_1:	Gear_Goto_N(); break;
				case MovTo_Z_2: Gear_Goto_R(); break;
			}
			break;
			
		case GEARS_ERR: //do nothing
			break;
	}
	*MovtoPostion = MovNone;//清除换挡动作//
}

/*************************************************************************************************************************
| NAME:
| PROTOTYPE:
| CALLED BY:
| PRECONDITIONS:
| INPUT PARAMETERS:
| RETURN VALUE:
|
| DESCRIPTION:
|
|*************************************************************************************************************************/
uint8_t ComparePhoIntValue(PhotoInt_ST* pPhoInt, const uint8_t* TableValue)
{
	uint8 i,j;
	uint8 idx = 0;
	uint8 CmpCnt = 0;
	uint8 PosOk = false;
	  
	for(i=0; i<5; i++)
	{
		if(pPhoInt->CommbValue.Value == TableValue[i])
		{
			pPhoInt->MachPosIdx = i;
			PosOk = true;
		}
	}
	return PosOk;
}
/*************************************************************************************************************************
当霍尔值与真值表不同时，记录该
b新真值表建立好后，就关闭诊断了。若霍尔再出现错误，就无法识别了 OK
c只允许同一个霍尔出现错误 OK
*************************************************************************************************************************/
uint8_t ScanHallSensor(void)
{
	uint8 i;
	uint8 idx = 0xff;
	uint8 CmpCnt = 0;
	uint8 unmatchedCnt = 0xff;
	uint8 match_value = 0x0;
	uint8 hall_value = PhoInt.CommbValue.Value & 0x0f;
	uint8_t err_hall_mask = 0;

    if(PhoInt.PreHallValue != hall_value)
    {
		PhoInt.PreHallValue = hall_value;
		//1. 默认真值表优先
		for(i=0; i<5; i++)
		{
			if(hall_value == PhotoIntValue[i])
			{
				PhoInt.MachPosIdx = i;
				PhoInt.ConfirmPosition[i] = 1;
				PhoInt.PassPositionFlag |= 1 << i;
				if(PhoInt.PassPositionFlag == 0x1F) memset(hall_fault, 0, 4);
				return true;
			}
		}
		//只有1个霍尔匹配不上时，放入缓存
		if(only_one_unmatched())
		{
			for(i=0; i<5; i++) 
			{
				if(PhoInt.ErrorVaule[i] == hall_value) break;
			}
			//新错误值填入ErrorVaule
			if(i == 5) //只记录1次
			{
				if(PhoInt.ErrPositionCode != hall_value) 
				{
					PhoInt.ErrorVaule[PhoInt.ErrorPositionCount] = hall_value;
					if(PhoInt.ErrorPositionCount < 5) PhoInt.ErrorPositionCount++;
				}
				if(PhoInt.ErrorPositionCount == 2) 
				{
					PhoInt.ErrSensorConfirmIdx = calc_error_sensor();
					hall_fault[PhoInt.ErrSensorConfirmIdx] = 1;
					/* record error sensor */
					if(PhoInt.ErrSensorConfirmIdx != 0xff) 
					{
						if(PhoInt.ErrSensorIndex == 0xff)
						{
							PhoInt.ErrSensorIndex = PhoInt.ErrSensorConfirmIdx;
						}
						if(PhoInt.ErrSensorConfirmIdx != PhoInt.ErrSensorIndex)
						{
							/* handle two error sensor  */
						}
					}
				}
			}
			//判断错误值是哪个postion
			if(PhoInt.ErrSensorIndex != 0xff)
			{			
				err_hall_mask = (~(1 << PhoInt.ErrSensorIndex)) & 0x0f;
				for(i=0; i<5; i++)//5 point
				{
					if((hall_value & err_hall_mask) == (PhotoIntValue[i] & err_hall_mask))
					{
						PhoInt.MachPosIdx = i;
						hall_fault[PhoInt.ErrSensorIndex] = 1;
						PhoInt.PassPositionFlag = 0; // &= ~(1 << i); 
						return true;
					}
				}
			}
		}
		return false;
    }
	return false;
}
uint8_t only_one_unmatched(void)
{
	uint8 i,j = 0;
	uint8 unmatchedCnt = {0};
	uint8 match_value = 0xff;
	uint8 mismatch_idx = 0xff;

	for(i = 0; i < 5; i++)//5个位置
	{
		unmatchedCnt = 0;
		match_value = (PhoInt.CommbValue.Value & 0x0f) ^ PhotoIntValue[i];
		if(match_value == 0U) 
		{
			PhoInt.MachPosIdx = i;
		}
		else
		{
			for(j = 0; j < 4; j++)//4个霍尔
			{
				if(match_value & (1 << j)) 
				{
					unmatchedCnt++;
					mismatch_idx = j; //不匹配的霍尔位号
				}
			}
			if(unmatchedCnt == 1) return 1;//break;
		}
	}
    return 0;//mismatch_idx;
}

uint8_t calc_error_sensor(void)
{
	uint8_t i,j;
	uint8 unmatchedCnt = {0};
	uint8 mismatch_idx1 = 0xff, mismatch_idx2 = 0xff;
	uint8 match_value1 = 0xff, match_value2 = 0xff;

	if((PhoInt.ErrorVaule[0] == 0xff) || (PhoInt.ErrorVaule[1] == 0xff)) 
		return 0xff;

	for(i=1; i<5; i++)
	{
		if(PhoInt.ConfirmPosition[i] == 1) continue; //不扫描Correct的position值
		match_value1 = PhoInt.ErrorVaule[0] ^ PhotoIntValue[i];

		if(CountOnes(match_value1, &mismatch_idx1) == 1)
		{
			for(j=1; j<5; j++)
			{
				if((j == i) || (PhoInt.ConfirmPosition[i] == 1)) continue;
				match_value2 = PhoInt.ErrorVaule[1] ^ PhotoIntValue[j];
				
				if(CountOnes(match_value2, &mismatch_idx2) == 1)
				{
					if(mismatch_idx1 ==  mismatch_idx2) {
						return mismatch_idx1;
					}
				}
			}
		}
	}
	return 0xff;
}

uint8_t CountOnes(uint8_t byte, uint8_t *idx)
{
	uint8_t count = 0;
	uint8_t position = 0;
	*idx = 0;

	while(byte){
		if(byte & 1) {
			count++;
			*idx = position;
		}
		byte >>= 1;
		position++;
		if(position == 4) break;
	}
	return count;
}
uint8_t IF_SwitchGetGearCurState(void)
{
	return MachSt.GearCurState;
}

uint8_t IF_GetShiftLevelPos(void)
{
	CanTx_ShiftPos = 0x0;
	if((IF_GetVehicleSpeed() > 53u) || (Switch[IG_SIG_INDEX].Flt != Press)) return CanTx_ShiftPos;
	switch(HandlePosition)
	{
		case POINT_0_RST: if(HandlePosFrmCnt == 0) CanTx_ShiftPos = 0x0; else HandlePosFrmCnt--; break;
		case POINT_1_POS: CanTx_ShiftPos = 0x2; HandlePosFrmCnt = 3; break;
		case POINT_2_POS: CanTx_ShiftPos = 0x3; HandlePosFrmCnt = 3; break;
		case POINT_1_NEG: CanTx_ShiftPos = 0x4; HandlePosFrmCnt = 3; break;
		case POINT_2_NEG: CanTx_ShiftPos = 0x5; HandlePosFrmCnt = 3; break;
		default : CanTx_ShiftPos = 0x0; break;
	}
	return CanTx_ShiftPos;
}
uint8_t IF_SwitchGetMechErrState(void)
{
	if((SwitchDiag.MechFault == 1) /*|| (PgearErrFlag == 1)*/) return 1;
	else return 0;
}

/* 霍尔开关信号 */
uint8_t IF_SwitchGetValue(void)
{
	return (IF_GetSwitchState(PHO_INT_1_INDEX) << 3 | (IF_GetSwitchState(PHO_INT_2_INDEX) << 2) | (IF_GetSwitchState(PHO_INT_3_INDEX) << 1) | IF_GetSwitchState(PHO_INT_4_INDEX));
}
/*************************************************************************************************************************
| NAME:
| PROTOTYPE:
| CALLED BY:
| PRECONDITIONS:
| INPUT PARAMETERS:
| RETURN VALUE:
|
|
| DESCRIPTION:
|
|*************************************************************************************************************************/
bool IF_GetSwitchState(uint8 SW_INDEX)
{
	if(SW_INDEX >= SWITCH_STRUCT_SIZE) return 0xff;
    if((Switch[SW_INDEX].Flt == Press)/*||(Switch[SW_INDEX].Flt == ReleaseFlt)*/)
    {
        return ON;
    }
    else
    {
        return OFF;
    }
}

/**
  * @brief  霍尔传感器故障.
  * @param  index 传感器位号
  *         HALL1_INDEX
  *         HALL2_INDEX
  *         HALL3_INDEX
  *         HALL4_INDEX
  * @retval uint8_t
  */
uint8_t GetSensorFault(uint8_t index)
{
	if(index > 3) return 0;
	return hall_fault[index];
}